﻿using System;
using System.Diagnostics;
using System.IO.Ports;
using System.Text.RegularExpressions;
using System.Threading;
using Coding4Fun.Cannon.Robot.Types;

namespace Coding4Fun.Cannon.Robot
{
	public class AmpFlow : IDisposable
	{
		private static SerialPort _com;

		#region constructors
		public AmpFlow(string comport)
			: this(comport, false)
		{
		}

		public AmpFlow(string comport, bool disableComport) : this()
		{
			_com = new SerialPort(comport, 9600, Parity.Even, 7, StopBits.One);

			DisableComport = disableComport;
		}

		// hiding default constructor
		private AmpFlow() { }
		#endregion
		#region idispose method
		public void Dispose()
		{
			if (_com == null)
				return;

			CloseComPort();
			_com.Dispose();
		}
		#endregion

		public bool DisableComport { get; set; }
		#region public methods
		public void Initialize()
		{
			//motorController.SetAcceleration(Core.Skateboard.Types.MotorTypes.AccelerationCurve.Acc_15_63);
			//motorController.SetMotorTuningAdjustment(Core.Skateboard.Types.MotorTypes.MotorTuningAdjustment.None);
		}


		/// <summary>
		/// 
		/// </summary>
		/// <param name="motorChannel"></param>
		/// <param name="powerRating">int between -127 and 127</param>
		/// <returns></returns>
		public bool SetMotorSpeed(RobotTypes.MotorChannel motorChannel, int powerRating)
		{
			string temp = "!";
			if (powerRating < 0)
			{
				temp += motorChannel.ToString().ToLower();
				powerRating *= -1; // needs to be between 0 and 127
			}
			else
				temp += motorChannel;
			
			if (powerRating > 127)// page 90 in manual, hex value of 00 to 7F which is 127
				powerRating = 127;

			temp += string.Format("{0:X2}", powerRating);

			write(temp);
			return true;
			//return successOnSet();
		}

		public MotorPowerReturn QueryPowerRatingApplied(RobotTypes.MotorChannel motorChannel)
		{
			if (DisableComport)
				return new MotorPowerReturn(new [] { 125, 125 });

			write("?v");
			return new MotorPowerReturn(getHexToIntValueFromController());
		}

		public MotorPowerReturn QueryAmpConsumed(RobotTypes.MotorChannel motorChannel)
		{
			if (DisableComport)
				return new MotorPowerReturn(new [] { 125, 125 }); 

			write("?a");
			return new MotorPowerReturn(getHexToIntValueFromController());
		}

		public BatteryVoltage QueryBatteryVoltage()
		{
			return new BatteryVoltage(new [] { 125, 125 }); 
			//if (DisableComport)
			//    return new BatteryVoltage(new int[] { 125, 125 }); 

			//write("?e");
			//// todo: fix this and make it consistant
			//string data;
			//while (!(data = getComResponse()).Contains("?e"))
			//{
			//}

			//Debug.WriteLine(data);
			//MatchCollection tempReturns = regex.Matches(data);
			//int[] returnValues = new int[tempReturns.Count];
			//for (int i = 0; i < returnValues.Length; i++)
			//    returnValues[i] = Convert.ToInt32(tempReturns[i].Value, 16);
			//return new BatteryVoltage(returnValues);
//			return new BatteryVoltage(getHexToIntValueFromController());
		}


		public TemperatureReturn QueryHeatSinkTempature()
		{
			write("?m");
			return new TemperatureReturn(getHexToIntValueFromController());
		}

		public bool SetMotorTuningAdjustment(RobotTypes.MotorTuningAdjustment tuningAdjustment)
		{
			return setParameterChange(RobotTypes.Parameter.MotorTuningAdjust, (int)tuningAdjustment, true);
		}

		public RobotTypes.MotorTuningAdjustment GetMotorTuningAdjustment()
		{
			return (RobotTypes.MotorTuningAdjustment)getParameter(RobotTypes.Parameter.MotorTuningAdjust);
		}

		public bool SetAcceleration(RobotTypes.AccelerationCurve accelerationCurve)
		{
			return setParameterChange(RobotTypes.Parameter.Acceleration, (int)accelerationCurve, true);
		}

		public RobotTypes.AccelerationCurve GetAcceleration()
		{
			return (RobotTypes.AccelerationCurve)getParameter(RobotTypes.Parameter.Acceleration);
		}

		public void ResetController()
		{
			write("%rrrrrr");
		}
		
		public void OpenComPort()
		{
			if (DisableComport)
				return;

			if (!_com.IsOpen)
			{
				_com.Open();

				for (int i = 0; i < 10; i++) // setting controller to RS232 (serial port)
					write("");

				Thread.Sleep(100);
				_com.ReadExisting();
			}
		}

		public void CloseComPort()
		{
			if (!_com.IsOpen)
				return;

			SetMotorSpeed(RobotTypes.MotorChannel.A, 0);
			SetMotorSpeed(RobotTypes.MotorChannel.B, 0);

			_com.Close();
		}
		#endregion
		#region private methods
		private bool setParameterChange(RobotTypes.Parameter param, int value, bool setParameter)
		{
			write("^" + string.Format("{0:X2}", param) + string.Format("{0:X2}", value));
			return (setParameter) ? applySetChanges() : successOnSet();
		}

		private int getParameter(RobotTypes.Parameter param)
		{
			write("^" + string.Format("{0:X2}", param));
			int[] data = getHexToIntValueFromController();

			return data[data.Length - 1];
		}

		private bool applySetChanges()
		{
			write("^FF");
			return successOnSet();
		}

		private bool successOnSet()
		{
			return hasValue("+");
		}

		private readonly Regex regex = new Regex("([A-Fa-f0-9]){1,2}", RegexOptions.Compiled);
		private int[] getHexToIntValueFromController()
		{
			string data = getComResponse();
			Debug.WriteLine(data);
			MatchCollection tempReturns = regex.Matches(data);
			int[] returnValues = new int[tempReturns.Count];
			for (int i = 0; i < returnValues.Length; i++)
				returnValues[i] = Convert.ToInt32(tempReturns[i].Value, 16);

			return returnValues;
		}

		private bool hasValue(string expectedReturn)
		{
			if (DisableComport)
				return true;

			string tempValue = getComResponse();
			
			return tempValue.IndexOf(expectedReturn) >= 0;
		}

		private string getComResponse()
		{
			string returnData = "";
			if (!DisableComport)
			{
				for (int i = 0; i < 10; i++)
				{
					returnData = _com.ReadExisting().Trim();

					if (!string.IsNullOrEmpty(returnData))
						break;
				}
			}

			return returnData;
		}

		private void write(string value)
		{
			if (!DisableComport)
				_com.WriteLine(value + Environment.NewLine);
		}

		#endregion
	}
}


